package estrategia;

import javax.vecmath.Point3d;

import comunicacion.Conector;

public class Utiles {
	
	private Conector entorno;
	
	public Utiles(Conector entorno) {
		this.entorno = entorno; 
	}
	
	/**
	 * Calcula la velocidad lineal necesaria segun el metodo de velocidades lineales de Lyapunov
	 * dadas las posiciones de origen (actual) y destino (a la que quiero llegar)
	 */
	double velocidadLinealLV(Point3d origen, Point3d destino) {
		double gamma = entorno.getFactorGamma();
		
		// Velocidad lineal de Lyapunov
		// En este caso es una modificacion, usamos la raiz de la distancia 
		// para tratar de suavizar la diferencia, achicando las grandes y agrandando las muy chicas
		return gamma * Math.sqrt(distanciaEuclidea(origen, destino));
	}
		
	
	/**
	 * Calcula la velocidad angular necesaria segun el metodo de velocidades lineales de Lyapunov
	 * dadas las posiciones de origen (actual) y destino (a la que quiero llegar)
	 */
	double velocidadAngularLV(Point3d origen, Point3d destino) {
		double gamma = entorno.getFactorGamma();
		double beta = entorno.getFactorBeta();
		double h = entorno.getFactorH();
		
		double theta = getAnguloTheta(origen, destino);
	
		double rotacionActual = origen.z;
		double alpha = getAnguloAlpha(theta, rotacionActual);
	
		double w = gamma * (Math.sin(alpha) + h*theta*Math.sin(alpha)/alpha + beta*alpha);
	
		return w;
	}
	
	/**
	 * Calcula la distancia euclidea entre dos puntos del plano (solamente considera x e y)
	 */
	double distanciaEuclidea(Point3d origen, Point3d destino) {
		double dist2 = Math.pow(Math.abs(origen.x - destino.x), 2) + Math.pow(Math.abs(origen.y - destino.y), 2);
		
		return Math.sqrt(dist2);
	}
	
	public double getAnguloTheta(Point3d origen, Point3d destino) {
		double catAdyacente = destino.x - origen.x;
		double catOpuesto = destino.y - origen.y;
		
		double theta = 0.0;
		
		if (catAdyacente == 0) {			
			theta = Math.PI/2 * Math.signum(catOpuesto);			
		} else {
			// Calculamos el angulo entre el punto de origen y destino, tomando como referencia
			// la recta que pasa por el punto de partida y tiene la direccion absoluta de angulo 0 del marco de ref
			theta = Math.atan(catOpuesto / catAdyacente);
			
			// Cuando estamos en los cuadrantes 1 y 2 (sin considerar rotacion), el angulo cambia
			if (destino.x < origen.x) {
				// El signo de theta es para saber si tenemos que restar o sumar pi
				double signoTheta = -1.0;
				if (theta != 0.0)
					signoTheta = Math.signum(theta);
				
				// Hay que ir "hacia atras"
				theta = theta - Math.PI*signoTheta;
			}
		}
		
		return theta;		
	}
	
	public double getAnguloAlpha(double theta, double rotacionActual /*, double rotacionDestino*/) {
		double alpha = theta - rotacionActual;
		
		// TODO FALTA TENER EN CUENTA EL ANGULO DE LLEGADA
		
		// Buscamos el menor angulo posible
		if (alpha < -Math.PI)
			alpha = alpha + 2*Math.PI;
		else if (alpha > Math.PI)
			alpha = alpha - 2*Math.PI;
		
		return alpha;
	}

	/** 
	 * Devuelve el cuadrante en el que se encuentra el punto (xOrig, yOrig) con respecto al 
	 * punto (xDest, yDest) si se considera una rotación (rDest) en los ejes 
	 * */	
	public int getCuadrante(Point3d origen, Point3d destino) {
		int cuadrante = 0;
		
		double theta = getAnguloTheta(origen, destino);
		double rDest = destino.z;
		
		if (theta <= rDest) {
			if (theta + Math.PI/2 <= rDest) {
				cuadrante = 1;
			} else {
				cuadrante = 2;
			}
		} else {
			if (theta - Math.PI/2 >= rDest) {
				cuadrante = 4;
			} else {
				cuadrante = 3;
			}
		}
		
		return cuadrante;
	}
	
	public double getDiferenciaAngulos(double a, double b) {
		double diferencia = 0;
		if (a >= 0 && b >=0 || a <= 0 && b <=0) {
			diferencia = Math.abs((a - b));
		} else if (a >= 0 && b < 0){
			if (a - Math.PI > b)
				diferencia = Math.PI - a + Math.PI + b;
			else
				diferencia = a - b;
		} else {
			if (b - Math.PI > a)
				diferencia = Math.PI - b + Math.PI + a;
			else
				diferencia = b - a;
		}
		
		return diferencia;
	}
}